﻿# coding=utf-8
# 2023/4/27: 首个记录，基于旧2020/9/28版本
# 2023/5/5: 枚举基于Enum
# 2023/10/11: 补充部分英文注释
# 2024/4/12: 修正插值时second_left/right_index未输出问题
# 2025/7/18: 修正significant赋值
# 2025/7/21: 使用类型注解。优化to_general_sample

import bi_common as bi
from math import sin, cos, atan
from enum import Enum
from typing import List, Any

class LineDescription(Enum):
    MODEL = 1  # Describe with model coefficients and consistent properties / 通过模型参数和一致的属性进行描述
    SEGMENTS = 2  # Describe with many segments and properties of each segment / 通过多段线段和各自的属性进行描述
    BOTH = 3  # Both model and segments are supported / Model和Segments双模式描述


class LineClass(Enum):
    GENERAL = 1  # 未指定
    DASHED = 2  # 虚线
    SOLID = 3  # 实线
    DOUBLE_DASHED = 4  # 双虚线
    DOUBLE_SOLID = 5  # 双实线
    SOLID_DASHED = 6  # 左实右虚
    DASHED_SOLID = 7  # 左虚右实
    CURB = 8  # 路沿
    GUARDRAIL = 9  # 护栏
    BARRIER = 10  # 障碍物边界


class LineColor(Enum):
    UNKNOWN = 1  # 未知
    WHITE = 2  # 白色
    YELLOW = 3  # 黄色
    RED = 4  # 红色
    BLUE = 5  # 蓝色
    ORANGE = 6  # 橙色
    GREEN = 7  # 绿色


class LineSegmentSolidProperty:
    def __init__(self):
        self.valid: bool = False  # Whether it's valid, False indicates virtual part / 是否有效，False表示为虚线段
        self.color: LineColor = LineColor.UNKNOWN  # Color of the line segment / 线段颜色
        self.width: float | None = None  # [m] Width of the line segment / 线段宽度


class LineSegment:
    def __init__(self):
        self.id: int = 0  # ID of line segment / 线段ID
        self.p1x: float = 0.0  # [m] x coordination of the start point / 线段首端点的x坐标
        self.p1y: float = 0.0  # [m] y coordination of the start point / 线段首端点的y坐标
        self.p1z: float | None = None  # [m] z coordination of the start point / 线段首端点的z坐标
        self.p2x: float = 0.0  # [m] x coordination of the end point / 线段末端点的x坐标
        self.p2y: float = 0.0  # [m] y coordination of the end point / 线段末端点的y坐标
        self.p2z: float | None = None  # [m] z coordination of the end point / 线段末端点的z坐标
        self.solid_property: LineSegmentSolidProperty = LineSegmentSolidProperty()  # Properties of solid line segment (Not for virtual part) / 实线段的属性（虚线段则为空）


class LaneLine:
    def __init__(self):
        self.id: int = 0  # ID of the lane line, zero as invalid / 车道线ID，0表示无效
        self.raw_id: int | None = None  # Raw ID of the lane line / 车道线原始ID
        self.confidence: float | None = None  # [%] Confidence of the lane line / 车道线置信度
        self.description: LineDescription = LineDescription.MODEL  # How to describe the lane line's curve / 车道线的描述方式
        self.classification: LineClass = LineClass.GENERAL  # Model description: Classification of lane line / Model模式：一致性车道线类型
        self.color: LineColor = LineColor.UNKNOWN  # Model description: Color of lane line / Model模式：一致性车道线颜色
        self.width: float | None = None  # [m] Model description: Line width of lane line / Model模式：一致性车道线宽度
        self.param0: float = 0.0  # Model description: Zero order coefficient of y-function of lane line / Model模式：车道线y方程0次方系数
        self.param1: float = 0.0  # Model description: First order coefficient of y-function of lane line / Model模式：车道线y方程1次方系数
        self.param2: float = 0.0  # Model description: Second order coefficient of y-function of lane line / Model模式：车道线y方程2次方系数
        self.param3: float = 0.0  # Model description: Third order coefficient of y-function of lane line / Model模式：车道线y方程3次方系数
        self.paramz0: float | None = None  # Model description: Zero order coefficient of z-function of lane line / Model模式：车道线z方程0次方系数
        self.paramz1: float | None = None  # Model description: First order coefficient of z-function of lane line / Model模式：车道线z方程1次方系数
        self.paramz2: float | None = None  # Model description: Second order coefficient of z-function of lane line / Model模式：车道线z方程2次方系数
        self.paramz3: float | None = None  # Model description: Third order coefficient of z-function of lane line / Model模式：车道线z方程3次方系数
        self.rear_end: float | None = None  # [m] Model description: X-axis coordination of read end of the lane line / Model模式：曲线后端点位置(x轴)
        self.front_end: float | None = None  # [m] Model description: X-axis coordination of front end of the lane line / Model模式：曲线前端点位置(x轴)
        self.segments: List[LineSegment] = []  # Segment description: List of line segments / Segments模式：线段列表

    def model_valid(self) -> bool:
        return self.description == LineDescription.MODEL or self.description == LineDescription.BOTH

    def segments_valid(self) -> bool:
        return self.description == LineDescription.SEGMENTS or self.description == LineDescription.BOTH


class LaneSensorSample:
    def __init__(self):
        self.time: float = 0.0  # [s] Time offset in session / Session内的相对时间
        self.lines: List[LaneLine] = []  # List of lane lines / 车道线列表
        self.first_left_index: int = -1  # Index of the first lane line to the left side, -1 indicated not exist / 左侧第一条车道线序号，-1表示不存在
        self.first_right_index: int = -1  # Index of the first lane line to the right side, -1 indicated not exist / 右侧第一条车道线序号，-1表示不存在
        self.second_left_index: int = -1  # Index of the second lane line to the left side, -1 indicated not exist / 左侧第二条车道线序号，-1表示不存在
        self.second_right_index: int = -1  # Index of the second lane line to the right side, -1 indicated not exist / 右侧第二条车道线序号，-1表示不存在
        self.lane_width: float | None = None  # [m] Current lane's width / 当前车道宽度
        self.lane_heading: float | None = None  # [deg] Current lane's orientation / 当前车道朝向
        self.lane_curvature: float | None = None  # [1/m] Current lane's curvature / 当前车道曲率
        self.lateral_velocity: float | None = None  # [m/s] Lateral speed to current lane / 当前车道横向速度
        self.center_departure: float | None = None  # [m] Central departure to current lane / 当前车道中央偏离距离
        self.rear_bound: float = 0.0  # [m] Rear bound of detect range / 传感器检测范围后边界
        self.front_bound: float = 30.0  # [m] Front bound of detect range / 传感器检测范围前边界
        self.vehicle_speed: float | None = None  # [kph] Subject vehicle's speed / 本车车速
        self.vehicle_curvature: float | None = None  # [1/m] Subject vehicle's turning curvature / 本车行驶曲率
        self.vehicle_ax: float | None = None  # [m/s²] Subject vehicle's longitudinal acceleration / 本车纵向加速度
        self.vehicle_width: float | None = None  # [m] Subject vehicle's width / 本车宽度
        self.vehicle_front_overhang: float | None = None  # [m] Subject vehicle's front overhang / 本车前悬
        self.vehicle_wheel_base: float | None = None  # [m] Subject vehicle's wheel base / 本车轴距

    # Calculate DTLC / 计算DTLC [m]
    def cal_dtlc(self, line: LaneLine) -> float | None:
        if not isinstance(line, LaneLine) or not line.model_valid() or self.vehicle_width is None or self.vehicle_front_overhang is None:
            return None
        is_left = line.param0 > 0
        x = -self.vehicle_front_overhang
        y = (line.param3 * x * x * x + line.param2 * x * x + line.param1 * x + line.param0) * (1 if is_left else -1)
        cos_theta = abs(cos(atan(line.param1)))
        return cos_theta * (y - self.vehicle_width * 0.5)

    # Calculate STLC / 计算STLC [m/s]
    def cal_stlc(self, line: LaneLine) -> float | None:
        if not isinstance(line, LaneLine) or not line.model_valid() or self.vehicle_speed is None:
            return None
        is_left = line.param0 > 0
        curv = self.vehicle_curvature if self.vehicle_curvature is not None else 0
        fo = self.vehicle_front_overhang if self.vehicle_front_overhang is not None else 0.9
        wb = self.vehicle_wheel_base if self.vehicle_wheel_base is not None else 2.8
        sin_theta = sin(atan(line.param1 - curv * (fo + wb)))
        return (-1 if is_left else 1) * sin_theta * self.vehicle_speed / 3.6

    # Calculate TTLC / 计算TTLC [s]
    def cal_ttlc(self, line: LaneLine) -> float | None:
        if not isinstance(line, LaneLine) or not line.model_valid() or self.vehicle_speed is None or self.vehicle_width is None or self.vehicle_front_overhang is None:
            return None
        if self.vehicle_speed <= 0:
            return None
        is_left = line.param0 > 0
        wb = self.vehicle_wheel_base if self.vehicle_wheel_base is not None else 2.8
        x0 = -self.vehicle_front_overhang - wb
        x = x0
        y = 0
        vx = self.vehicle_speed / 3.6
        loop_times = 1000 if vx < 5 else int(50.0 / vx * 100)  # No output if crossing beyond 50m (Can't guarantee precision) / 超过50米越线则不输出（精度无法保证）
        rx = wb
        ry = (0.5 if is_left else -0.5) * self.vehicle_width
        for i in range(0, loop_times + 1):
            k = (self.vehicle_curvature * (x - x0)) if self.vehicle_curvature is not None else 0
            cosa = 1.0 / abs(1 + k)
            sina = cosa * k
            tx = x + cosa * rx - sina * ry
            ty = y + sina * rx + cosa * ry
            ly = line.param3 * tx * tx * tx + line.param2 * tx * tx + line.param1 * tx + line.param0
            passed = False
            if is_left:
                if ly <= ty:
                    passed = True
            else:
                if ly >= ty:
                    passed = True
            if passed:
                if i == 0:
                    return None
                else:
                    return 0.01 * i
            if self.vehicle_ax is not None:
                vx = vx + 0.01 * self.vehicle_ax
            if vx <= 0:
                break
            x = x + vx * 0.01 * cosa
            y = y + vx * 0.01 * sina
        return None

    # Convert to general sample for output / 转通用样本，用于样本输出
    def to_general_sample(self, channel: int) -> Any:
        output = bi.agency.create_general_sample()
        output.protocol = "lane-sensor-sample-v4@" + str(channel)
        segment_count = 0
        segment_offsets = []
        segment_sizes = []
        for line in self.lines:
            segment_offsets.append(segment_count)
            segment_sizes.append(len(line.segments))
            segment_count += len(line.segments)
        output.time = self.time

        total_length = 23 + len(self.lines) * 27 + segment_count * 12
        output.significant = total_length
        
        values: List[float | str | None] = [None] * total_length
        values[0] = len(self.lines)
        values[1] = segment_count
        values[2] = self.lateral_velocity
        values[3] = self.center_departure
        values[4] = self.lane_width
        values[5] = self.lane_heading
        values[6] = self.lane_curvature
        values[7] = self.rear_bound
        values[8] = self.front_bound
        values[9] = self.first_left_index if self.first_left_index >= 0 else None
        values[10] = self.first_right_index if self.first_right_index >= 0 else None
        values[11] = self.second_left_index if self.second_left_index >= 0 else None
        values[12] = self.second_right_index if self.second_right_index >= 0 else None
        values[13] = self.vehicle_speed
        values[14] = self.vehicle_curvature
        values[15] = self.vehicle_ax
        values[16] = self.vehicle_width
        values[17] = self.vehicle_front_overhang
        values[18] = self.vehicle_wheel_base

        i = 0
        for line in self.lines:
            base_index = 23 + i * 27
            values[base_index] = line.id
            values[base_index + 1] = line.raw_id
            values[base_index + 2] = line.confidence
            values[base_index + 3] = line.description.value
            if line.model_valid():
                values[base_index + 4] = line.classification.value
                values[base_index + 5] = line.color.value
                values[base_index + 6] = line.width
                values[base_index + 7] = line.param0
                values[base_index + 8] = line.param1
                values[base_index + 9] = line.param2
                values[base_index + 10] = line.param3
                values[base_index + 11] = 1 if line.paramz0 is not None else 0
                values[base_index + 12] = line.paramz0
                values[base_index + 13] = line.paramz1
                values[base_index + 14] = line.paramz2
                values[base_index + 15] = line.paramz3
                values[base_index + 16] = line.rear_end
                values[base_index + 17] = line.front_end
            if line.segments_valid():
                values[base_index + 25] = segment_offsets[i]
                values[base_index + 26] = segment_sizes[i]
            i += 1
        
        output.values = values
        return output


def _conv_lane_sensor_sample_v3(gs: Any) -> LaneSensorSample | None:
    values_count = len(gs.values)
    if values_count < 17:
        return None
    line_count = int(gs.values[0]) if gs.values[0] is not None else 0
    if values_count != 17 + line_count * 15:
        return None
    output = LaneSensorSample()
    output.time = gs.time
    output.lateral_velocity = float(gs.values[1]) if gs.values[1] is not None else None
    output.center_departure = float(gs.values[2]) if gs.values[2] is not None else None
    output.lane_width = float(gs.values[3]) if gs.values[3] is not None else None
    output.lane_heading = float(gs.values[4]) if gs.values[4] is not None else None
    output.lane_curvature = float(gs.values[5]) if gs.values[5] is not None else None
    output.vehicle_speed = float(gs.values[6]) if gs.values[6] is not None else None
    output.vehicle_curvature = float(gs.values[7]) if gs.values[7] is not None else None
    output.vehicle_ax = float(gs.values[8]) if gs.values[8] is not None else None
    output.vehicle_width = float(gs.values[9]) if gs.values[9] is not None else None
    output.vehicle_front_overhang = float(gs.values[10]) if gs.values[10] is not None else None
    output.rear_bound = float(gs.values[11]) if gs.values[11] is not None else 0.0
    output.front_bound = float(gs.values[12]) if gs.values[12] is not None else 30.0
    output.first_left_index = int(gs.values[13]) if gs.values[13] is not None else -1
    output.first_right_index = int(gs.values[14]) if gs.values[14] is not None else -1
    output.second_left_index = int(gs.values[15]) if gs.values[15] is not None else -1
    output.second_right_index = int(gs.values[16]) if gs.values[16] is not None else -1
    for i in range(0, line_count):
        line = LaneLine()
        b = 17 + 15 * i
        line.id = int(gs.values[b] if gs.values[b] is not None else 0)
        line.raw_id = int(gs.values[b + 1]) if gs.values[b + 1] is not None else None
        line.classification = LineClass(int(gs.values[b + 2])) if gs.values[b + 2] is not None else LineClass.GENERAL
        line.color = LineColor(int(gs.values[b + 3])) if gs.values[b + 3] is not None else LineColor.UNKNOWN
        line.confidence = float(gs.values[b + 4]) if gs.values[b + 4] is not None else None
        line.width = float(gs.values[b + 5]) if gs.values[b + 5] is not None else None
        line.param0 = float(gs.values[b + 6]) if gs.values[b + 6] is not None else 0.0
        line.param1 = float(gs.values[b + 7]) if gs.values[b + 7] is not None else 0.0
        line.param2 = float(gs.values[b + 8]) if gs.values[b + 8] is not None else 0.0
        line.param3 = float(gs.values[b + 9]) if gs.values[b + 9] is not None else 0.0
        line.rear_end = float(gs.values[b + 10]) if gs.values[b + 10] is not None else None
        line.front_end = float(gs.values[b + 11]) if gs.values[b + 11] is not None else None
        output.lines.append(line)
    return output


def _conv_lane_sensor_sample_v4(gs: Any) -> LaneSensorSample | None:
    values_count = len(gs.values)
    if values_count < 23:
        return None
    line_count = int(gs.values[0]) if gs.values[0] is not None else 0
    segment_count = int(gs.values[1]) if gs.values[1] is not None else 0
    if values_count != 23 + line_count * 27 + segment_count * 12:
        return None
    output = LaneSensorSample()
    output.time = gs.time
    output.lateral_velocity = float(gs.values[2]) if gs.values[2] is not None else None
    output.center_departure = float(gs.values[3]) if gs.values[3] is not None else None
    output.lane_width = float(gs.values[4]) if gs.values[4] is not None else None
    output.lane_heading = float(gs.values[5]) if gs.values[5] is not None else None
    output.lane_curvature = float(gs.values[6]) if gs.values[6] is not None else None
    output.rear_bound = float(gs.values[7]) if gs.values[7] is not None else 0.0
    output.front_bound = float(gs.values[8]) if gs.values[8] is not None else 30.0
    output.first_left_index = int(gs.values[9]) if gs.values[9] is not None else -1
    output.first_right_index = int(gs.values[10]) if gs.values[10] is not None else -1
    output.second_left_index = int(gs.values[11]) if gs.values[11] is not None else -1
    output.second_right_index = int(gs.values[12]) if gs.values[12] is not None else -1
    output.vehicle_speed = float(gs.values[13]) if gs.values[13] is not None else None
    output.vehicle_curvature = float(gs.values[14]) if gs.values[14] is not None else None
    output.vehicle_ax = float(gs.values[15]) if gs.values[15] is not None else None
    output.vehicle_width = float(gs.values[16]) if gs.values[16] is not None else None
    output.vehicle_front_overhang = float(gs.values[17]) if gs.values[17] is not None else None
    output.vehicle_wheel_base = float(gs.values[18]) if gs.values[18] is not None else None
    seg_base = 23 + 27 * line_count
    for i in range(0, line_count):
        line = LaneLine()
        b = 23 + 27 * i
        line.id = int(gs.values[b] if gs.values[b] is not None else 0)
        line.raw_id = int(gs.values[b + 1]) if gs.values[b + 1] is not None else None
        line.confidence = float(gs.values[b + 2]) if gs.values[b + 2] is not None else None
        line.description = LineDescription(int(gs.values[b + 3])) if gs.values[b + 3] is not None else LineDescription.MODEL
        if line.model_valid():
            line.classification = LineClass(int(gs.values[b + 4])) if gs.values[b + 4] is not None else LineClass.GENERAL
            line.color = LineColor(int(gs.values[b + 5])) if gs.values[b + 5] is not None else LineColor.UNKNOWN
            line.width = float(gs.values[b + 6]) if gs.values[b + 6] is not None else None
            line.param0 = float(gs.values[b + 7]) if gs.values[b + 7] is not None else 0.0
            line.param1 = float(gs.values[b + 8]) if gs.values[b + 8] is not None else 0.0
            line.param2 = float(gs.values[b + 9]) if gs.values[b + 9] is not None else 0.0
            line.param3 = float(gs.values[b + 10]) if gs.values[b + 10] is not None else 0.0
            if gs.values[b + 11] == 1 and gs.values[b + 12] is not None and gs.values[b + 13] is not None and gs.values[b + 14] is not None and gs.values[b + 15] is not None:
                line.paramz0 = float(gs.values[b + 12])
                line.paramz1 = float(gs.values[b + 13])
                line.paramz2 = float(gs.values[b + 14])
                line.paramz3 = float(gs.values[b + 15])
            line.rear_end = float(gs.values[b + 16]) if gs.values[b + 16] is not None else None
            line.front_end = float(gs.values[b + 17]) if gs.values[b + 17] is not None else None
        if line.segments_valid():
            seg_offset = int(gs.values[b + 25])
            seg_size = int(gs.values[b + 26])
            for n in range(0, seg_size):
                seg = LineSegment()
                seg_b = seg_base + 12 * (seg_offset + n)
                seg.id = int(gs.values[seg_b]) if gs.values[seg_b] is not None else 0
                seg.p1x = float(gs.values[seg_b + 1]) if gs.values[seg_b + 1] is not None else 0.0
                seg.p1y = float(gs.values[seg_b + 2]) if gs.values[seg_b + 2] is not None else 0.0
                seg.p1z = float(gs.values[seg_b + 3]) if gs.values[seg_b + 3] is not None else None
                seg.p2x = float(gs.values[seg_b + 4]) if gs.values[seg_b + 4] is not None else 0.0
                seg.p2y = float(gs.values[seg_b + 5]) if gs.values[seg_b + 5] is not None else 0.0
                seg.p2z = float(gs.values[seg_b + 6]) if gs.values[seg_b + 6] is not None else None
                seg.solid_property.valid = gs.values[seg_b + 7] == 1 if gs.values[seg_b + 7] is not None else False
                if seg.solid_property.valid is True:
                    seg.solid_property.color = LineColor(int(gs.values[seg_b + 8])) if gs.values[seg_b + 8] is not None else LineColor.UNKNOWN
                    seg.solid_property.width = float(gs.values[seg_b + 9]) if gs.values[seg_b + 9] is not None else None
                line.segments.append(seg)
        output.lines.append(line)
    return output


def _interpolate_lane_sensor_sample(s1: LaneSensorSample, w1: float, s2: LaneSensorSample, w2: float) -> LaneSensorSample:
    output = LaneSensorSample()
    output.time = bi.time
    if s1.lateral_velocity is not None and s2.lateral_velocity is not None:
        output.lateral_velocity = s1.lateral_velocity * w1 + s2.lateral_velocity * w2
    if s1.center_departure is not None and s2.center_departure is not None:
        output.center_departure = s1.center_departure * w1 + s2.center_departure * w2
    if s1.lane_width is not None and s2.lane_width is not None:
        output.lane_width = s1.lane_width * w1 + s2.lane_width * w2
    if s1.lane_heading is not None and s2.lane_heading is not None:
        output.lane_heading = s1.lane_heading * w1 + s2.lane_heading * w2
    if s1.lane_curvature is not None and s2.lane_curvature is not None:
        output.lane_curvature = s1.lane_curvature * w1 + s2.lane_curvature * w2
    output.rear_bound = s1.rear_bound if w1 > w2 else s2.rear_bound
    output.front_bound = s1.front_bound if w1 > w2 else s2.front_bound
    output.vehicle_width = s1.vehicle_width
    output.vehicle_wheel_base = s1.vehicle_wheel_base
    output.vehicle_front_overhang = s1.vehicle_front_overhang
    if s1.vehicle_speed is not None and s2.vehicle_speed is not None:
        output.vehicle_speed = s1.vehicle_speed * w1 + s2.vehicle_speed * w2
    if s1.vehicle_curvature is not None and s2.vehicle_curvature is not None:
        output.vehicle_curvature = s1.vehicle_curvature * w1 + s2.vehicle_curvature * w2
    if s1.vehicle_ax is not None and s2.vehicle_ax is not None:
        output.vehicle_ax = s1.vehicle_ax * w1 + s2.vehicle_ax * w2
    first_left_id = -1
    if s1.first_left_index >= 0 and s2.first_left_index >= 0 and s1.lines[s1.first_left_index].id == s2.lines[s2.first_left_index].id:
        first_left_id = s1.lines[s1.first_left_index].id
    first_right_id = -1
    if s1.first_right_index >= 0 and s2.first_right_index >= 0 and s1.lines[s1.first_right_index].id == s2.lines[s2.first_right_index].id:
        first_right_id = s1.lines[s1.first_right_index].id
    second_left_id = -1
    if s1.second_left_index >= 0 and s2.second_left_index >= 0 and s1.lines[s1.second_left_index].id == s2.lines[s2.second_left_index].id:
        second_left_id = s1.lines[s1.second_left_index].id
    second_right_id = -1
    if s1.second_right_index >= 0 and s2.second_right_index >= 0 and s1.lines[s1.second_right_index].id == s2.lines[s2.second_right_index].id:
        second_right_id = s1.lines[s1.second_right_index].id
    index = 0
    for o1 in s1.lines:
        id = o1.id
        for o2 in s2.lines:
            if o2.id != id:
                continue
            if first_left_id == id:
                output.first_left_index = index
            if first_right_id == id:
                output.first_right_index = index
            if second_left_id == id:
                output.second_left_index = index
            if second_right_id == id:
                output.second_right_index = index
            line = LaneLine()
            line.id = id
            line.raw_id = o1.raw_id
            if o1.confidence is not None and o2.confidence is not None:
                line.confidence = float(o1.confidence * w1 + o2.confidence * w2)
            line.description = o1.description
            if line.model_valid():
                line.classification = o1.classification if w1 > w2 else o2.classification
                line.color = o1.color if w1 > w2 else o2.color
                if o1.width is not None and o2.width is not None:
                    line.width = float(o1.width * w1 + o2.width * w2)
                line.param0 = o1.param0 * w1 + o2.param0 * w2
                line.param1 = o1.param1 * w1 + o2.param1 * w2
                line.param2 = o1.param2 * w1 + o2.param2 * w2
                line.param3 = o1.param3 * w1 + o2.param3 * w2
                if o1.paramz0 is not None and o2.paramz0 is not None and o1.paramz1 is not None and o2.paramz1 is not None and o1.paramz2 is not None and o2.paramz2 is not None and o1.paramz3 is not None and o2.paramz3 is not None:
                    line.paramz0 = o1.paramz0 * w1 + o2.paramz0 * w2
                    line.paramz1 = o1.paramz1 * w1 + o2.paramz1 * w2
                    line.paramz2 = o1.paramz2 * w1 + o2.paramz2 * w2
                    line.paramz3 = o1.paramz3 * w1 + o2.paramz3 * w2
                if o1.rear_end is not None and o2.rear_end is not None:
                    line.rear_end = float(o1.rear_end * w1 + o2.rear_end * w2)
                if o1.front_end is not None and o2.front_end is not None:
                    line.front_end = float(o1.front_end * w1 + o2.front_end * w2)
            if line.segments_valid():
                for seg1 in o1.segments:
                    sid = seg1.id
                    for seg2 in o2.segments:
                        if seg2.id != id:
                            continue
                        seg = LineSegment()
                        seg.id = sid
                        seg.p1x = seg1.p1x * w1 + seg2.p1x * w2
                        seg.p1y = seg1.p1y * w1 + seg2.p1y * w2
                        if seg1.p1z is not None and seg2.p1z is not None:
                            seg.p1z = seg1.p1z * w1 + seg2.p1z * w2
                        seg.p2x = seg1.p2x * w1 + seg2.p2x * w2
                        seg.p2y = seg1.p2y * w1 + seg2.p2y * w2
                        if seg1.p2z is not None and seg2.p2z is not None:
                            seg.p2z = seg1.p2z * w1 + seg2.p2z * w2
                        seg.solid_property = seg1.solid_property if w1 > w2 else seg2.solid_property
                        line.segments.append(seg)
            output.lines.append(line)
            index += 1
    return output


# Query LaneSensorSample for input / 获取LaneSensorSample，用于样本输入
def get_lane_sensor_sample(channel: int) -> LaneSensorSample | None:
    s1 = None
    s2 = None
    w1 = 0.0
    w2 = 0.0
    protocol_id_v3 = 'lane-sensor-sample-v3@' + str(channel)
    protocol_id_v4 = 'lane-sensor-sample-v4@' + str(channel)
    if protocol_id_v4 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v4]
        s1 = _conv_lane_sensor_sample_v4(pair.sample1)
        w1 = pair.weight1
        s2 = _conv_lane_sensor_sample_v4(pair.sample2)
        w2 = pair.weight2
    elif protocol_id_v3 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v3]
        s1 = _conv_lane_sensor_sample_v3(pair.sample1)
        w1 = pair.weight1
        s2 = _conv_lane_sensor_sample_v3(pair.sample2)
        w2 = pair.weight2
    if s1 is not None and s2 is not None:
        return _interpolate_lane_sensor_sample(s1, w1, s2, w2)
    return None
